import os
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory

def generate_launch_description():
    # 定位到功能包的地址
    pkg_share = get_package_share_directory('learning_gazebo')
    cartographer_config_dir = os.path.join(pkg_share, 'config')

    # ===================== 声明启动参数 ==============================
    use_sim_time = DeclareLaunchArgument(
        'use_sim_time',
        default_value='true',
        description='Use simulation (Gazebo) clock if true'
    )
    
    resolution = DeclareLaunchArgument(
        'resolution',
        default_value='0.05',
        description='Resolution of the occupancy grid'
    )
    
    publish_period_sec = DeclareLaunchArgument(
        'publish_period_sec',
        default_value='1.0',
        description='Occupancy grid publish period'
    )
    
    configuration_directory = DeclareLaunchArgument(
        'configuration_directory',
        default_value=cartographer_config_dir,
        description='Full path to the Cartographer config directory'
    )
    
    configuration_basename = DeclareLaunchArgument(
        'configuration_basename',
        default_value='qingzhou_2d.lua',
        description='Name of Cartographer config file'
    )

    # ===================== 声明节点 ================================
    cartographer_node = Node(
        package='cartographer_ros',
        executable='cartographer_node',
        name='cartographer_node',
        output='screen',
        parameters=[{'use_sim_time': LaunchConfiguration('use_sim_time')}],
        arguments=[
            '-configuration_directory', LaunchConfiguration('configuration_directory'),
            '-configuration_basename', LaunchConfiguration('configuration_basename')
        ])

    occupancy_grid_node = Node(
        package='cartographer_ros',
        executable='cartographer_occupancy_grid_node',
        name='cartographer_occupancy_grid_node',
        output='screen',
        parameters=[
            {'use_sim_time': LaunchConfiguration('use_sim_time')},
            {'resolution': LaunchConfiguration('resolution')},
            {'publish_period_sec': LaunchConfiguration('publish_period_sec')}
        ])

    rviz_node = Node(
        package='rviz2',
        executable='rviz2',
        name='rviz2',
        parameters=[{'use_sim_time': LaunchConfiguration('use_sim_time')}],
        output='screen',
        # 添加RViz配置文件的路径
      #  arguments=['-d', os.path.join(pkg_share, 'config', 'slam.rviz')]
    )

    # ===================== 定义启动文件 =============================
    ld = LaunchDescription()
    ld.add_action(use_sim_time)
    ld.add_action(resolution)
    ld.add_action(publish_period_sec)
    ld.add_action(configuration_directory)
    ld.add_action(configuration_basename)
    ld.add_action(cartographer_node)
    ld.add_action(occupancy_grid_node)
    ld.add_action(rviz_node)

    return ld